#include <dispatch_system/custom_movepatterns.h>

namespace moying
{
namespace navigation
{
    CustomMovePatterns::CustomMovePatterns(MoyingMap* moyingmap)
    :moyingmap_(moyingmap)
    {
        if(moyingmap == nullptr)
            return;
        MapDimension d = moyingmap_->getDimensions();
        move_patterns_on_map_.resize(d.x);
        for(int x = 0; x < move_patterns_on_map_.size(); x++)
        {
            move_patterns_on_map_[x].resize(d.y);
            for(int y = 0; y < move_patterns_on_map_[x].size(); y++)
            {
                move_patterns_on_map_[x][y].resize(d.theta);
                for(int th = 0; th < 4; th++)
                    move_patterns_on_map_[x][y][th].clear();
            }
        }
    }

    CustomMovePatterns::~CustomMovePatterns()
    {

    }

    bool CustomMovePatterns::load(const std::string &file)
    {

    }

    bool CustomMovePatterns::addCustomMove(int x, int y, int th, MovePatternState sm, MovePatternsPtr &custom_moves)
    {
        if(moyingmap_ == nullptr)
            return false;
        if(!moyingmap_->isOnMap(x, y, th))
            return false;
        
        if(move_patterns_on_map_[x][y][th].find(sm) != move_patterns_on_map_[x][y][th].end())
            WARNING("Overwite custom move patterns at ("<<x<<","<<y<<") theta="<<th<<" sm="<<sm);
        move_patterns_on_map_[x][y][th].emplace(sm, custom_moves);

        if(moyingmap_->getTimedMap()->at(x)[y] == nullptr)
            moyingmap_->addTimeList(x, y);
        return true;
    }

    bool CustomMovePatterns::hasCustomMove(int x, int y, int th, MovePatternState sm)
    {
        return false;
    }

    MovePatternsPtr CustomMovePatterns::getCustomMove(int x, int y, int th, MovePatternState sm,int search_theta_num)
    {
        // printf("getCustomMove x:%d y:%d th:%d sm:%d num:%d\n",x,y,(int)th,int(sm),search_theta_num);
        if(moyingmap_ == nullptr){
            return nullptr;
        }

        if(!moyingmap_->isOnMap(x, y, th)){
            return nullptr;
        }

        if(move_patterns_on_map_[x][y][th].find(sm) != move_patterns_on_map_[x][y][th].end()){
            auto move_patterns = move_patterns_on_map_[x][y][th][sm];
            if(search_theta_num == 8){
                return move_patterns;
            }else if(search_theta_num == 4){
                //4个方向搜索时,有效的运动模式也只能4个方向旋转
                auto valid_patterns = std::make_shared<MovePatterns>();
                if(th == 1 || th == 3 || th == 5 || th == 7){
                    for(auto type : *move_patterns){
                        auto item = Vehicle::getMovePattern(type);
                        if( item.dir_change == 1 || item.dir_change == 3 || item.dir_change == -1 || item.dir_change == -3){
                            valid_patterns->emplace_back(type);
                        }
                    }
                }else{
                    for(auto type : *move_patterns){
                        auto item = Vehicle::getMovePattern(type);
                        if(item.dir_change == 0 || item.dir_change == 2 || item.dir_change == 4 || item.dir_change == -2 || item.dir_change == -4){
                            valid_patterns->emplace_back(type);
                        }
                    }
                }
                
                return valid_patterns;
            }
        }
        
        return nullptr;
    }
}
}